import time
import numpy as np
import cv2
from src.actions import SetServo, Stop, Start, TurnLeft, TurnRight, Advance
from src.scenes.base_scene import BaseScene
from src.utils import log


def extract_yellow_region(image):
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

    lower_yellow = np.array([24, 100, 165])
    upper_yellow = np.array([30, 180, 225])

    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

    yellow_region = cv2.bitwise_and(image, image, mask=mask)

    gray = cv2.cvtColor(yellow_region, cv2.COLOR_BGR2GRAY)

    _, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY)

    return binary


def getEdgeImg(binary_img):
    canny_lth = 50
    canny_hth = 100
    edgeImg = cv2.Canny(binary_img, canny_lth, canny_hth)
    return edgeImg


# def roi_mask(img, corner_points):
#     mask = np.zeros_like(img)
#     cv2.fillPoly(mask, corner_points, 255)
#     masked_img = cv2.bitwise_and(img, mask)
#     return masked_img
def roi_mask(img, corner_points):
    # print("Image shape:", img.shape)
    # print("ROI corner points:", corner_points)

    mask = np.zeros_like(img)
    cv2.fillPoly(mask, corner_points, 255)
    masked_img = cv2.bitwise_and(img, mask)
    return masked_img


def process_image(image):
    rows, cols = image.shape

    for i in range(rows):
        for j in range(cols - 1, -1, -1):

            if image[i][j] != 0:

                for k in range(j, max(-1, j - 3), -1):
                    if k >= 0:
                        image[i][k] = 0
                break

    return image


def getRLaneLine(roi_edges):
    rho = 1
    theta = np.pi / 180
    threshold = 10
    min_line_len = 20
    max_line_gap = 5

    lines = cv2.HoughLinesP(roi_edges, rho, theta, threshold, minLineLength=min_line_len, maxLineGap=max_line_gap)

    if lines is not None:
        filtered_lines = []
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1)
            if slope > 0:  # filer the right lines
                filtered_lines.append(line)

        if filtered_lines:

            longest_line = max(filtered_lines, key=lambda x: np.linalg.norm(x[0][0:2] - x[0][2:4]))
            x1, y1, x2, y2 = longest_line[0]

            slope = (y2 - y1) / (x2 - x1)
            b = y2 - slope * x2
            return b, slope
        else:
            return -1.1
    else:
        return -1


# def get_x0(img_path):
#     imgOri = cv2.imread(img_path)
#     small_img = cv2.resize(imgOri, (int(imgOri.shape[1] / 4), int(imgOri.shape[0] / 4)))
#     roi_binary = extract_yellow_region(small_img)
#     edges = getEdgeImg(roi_binary)
#
#     rows, cols = edges.shape
#     points = np.array([[(0, rows), (0, 90), (319, 90), (cols, rows)]])
#
#     roi_edges = roi_mask(edges, points)
#
#     roi_edges = process_image(roi_edges)
#
#     b, slope = getRLaneLine(roi_edges)
#     x0 = (int)((179 - b) / slope)
#     print('slope:', slope)
#     print('b:', b)
#     print('x0:', x0)


def get_cur_offset_x(imgOri):
    cv2.imwrite('/home/HwHiAiUser/Car/capture/2.jpg', imgOri)
    small_img = cv2.resize(imgOri, (int(imgOri.shape[1] / 4), int(imgOri.shape[0] / 4)))
    roi_binary = extract_yellow_region(small_img)
    edges = getEdgeImg(roi_binary)

    rows, cols = edges.shape
    points = np.array([[(0, rows), (0, rows // 2), (cols, rows // 2), (cols, rows)]], dtype=np.int32)

    roi_edges = roi_mask(edges, points)
    roi_edges = process_image(roi_edges)

    result = getRLaneLine(roi_edges)

    # �ж��Ƿ�Ϊ�Ϸ������tuple�������򷵻�0
    if not isinstance(result, tuple) or len(result) != 2:
        return 0

    b, slope = result

    if slope == 0 or slope is None or b is None:
        return 0

    try:
        cur_pos = int((rows - b) / slope)
    except Exception:
        return 0

    delta_X = cur_pos - 305
    return delta_X * 4



class LF(BaseScene):
    def __init__(self, memory_name, camera_info, msg_queue):
        super().__init__(memory_name, camera_info, msg_queue)
        # self.net = None
        self.forward_spd = 29

    def init_state(self):
        log.info(f'start init {self.__class__.__name__}')

        self.ctrl.execute(SetServo(servo=[97, 30]))
        return False

    def loop(self):
        ret = self.init_state()
        if ret:
            log.error(f'{self.__class__.__name__} init failed.')
            return
        frame = np.ndarray((self.height, self.width, 3), dtype=np.uint8, buffer=self.broadcaster.buf)
        log.info(f'{self.__class__.__name__} loop start')
        self.ctrl.execute(Start())
        try:
            while True:
                if self.stop_sign.value:
                    break
                if self.pause_sign.value:
                    continue
                start = time.time()
                img_bgr = frame.copy()
                cv2.imwrite('/home/HwHiAiUser/Car/capture/1.jpg', img_bgr)
                # bgr_image = cv2.cvtColor(img_bgr, cv2.COLOR_RGB2BGR)
                x_offset = get_cur_offset_x(img_bgr)

                log.info(f'x_offset: {x_offset}')

                if -60 < x_offset <= 0:
                    self.ctrl.execute(Advance(speed=self.forward_spd))
                if 0 < x_offset <= 60:
                    self.ctrl.execute(TurnRight(degree=0.1))
                elif 90 < x_offset <= 120:
                    self.ctrl.execute(TurnRight(degree=0.13))
                elif 120 < x_offset <= 150:
                    self.ctrl.execute(TurnRight(degree=0.16))
                elif 150 < x_offset <= 180:
                    self.ctrl.execute(TurnRight(degree=0.20))
                elif 180 < x_offset <= 240:
                    self.ctrl.execute(TurnRight(degree=0.28))
                elif 240 < x_offset <= 300:
                    self.ctrl.execute(TurnRight(degree=0.36))
                elif 300 < x_offset:
                    self.ctrl.execute(TurnRight(degree=0.45))
                elif -90 < x_offset <= -60:
                    self.ctrl.execute(TurnLeft(degree=0.1))
                elif -120 < x_offset <= -90:
                    self.ctrl.execute(TurnLeft(degree=0.13))
                elif -150 < x_offset <= -120:
                    self.ctrl.execute(TurnLeft(degree=0.16))
                elif -180 < x_offset <= -150:
                    self.ctrl.execute(TurnLeft(degree=0.20))
                elif -240 < x_offset <= -180:
                    self.ctrl.execute(TurnLeft(degree=0.28))
                elif -300 < x_offset <= -240:
                    self.ctrl.execute(TurnLeft(degree=0.36))
                elif x_offset <= -300:
                    self.ctrl.execute(TurnLeft(degree=0.1))


                log.info(f'LF cost {time.time() - start}')
        except KeyboardInterrupt:
            self.ctrl.execute(Stop())
